clear
clc

%% Parameters
Mlast = 3.71;
Mlasty = Mlast / 2;
g = 9.82;
gaffel = 16.3*10^(-3);
%lbar = 0.4;
lbar = 0.638;
%lbar = 1.2;
Jtotalx = 0.1;
Jgeary = 0.71*10^(-3);
Btotalx = 0.257;
Bgeary = 2.34*10^(-3);
Rex = 0.43;
Rey = 0.43;
Kex = 12.6*10^(-3);
Key = 12.6*10^(-3);
r5xNx = 3.88*10^(-3);
r5yNy = 2.08*10^(-3);

%% Constants

K1 = gaffel / (Mlast * lbar^2);
K2 = g / lbar;
K3 = 1 / lbar;
K4 = (Jtotalx * Rex) / Kex;
K5 = (Btotalx * Rex * r5xNx + Kex^2) / (Kex * r5xNx);
K6 = (Mlast * Rex * r5xNx * g) / Kex;
K7 = (Jgeary * Rey + Mlasty * Rey * r5yNy^2) / (Key * r5yNy);
K8 = (Bgeary * (Rey / Key) + Key) / (r5yNy);

%% System

% A = [0 1 0 0 0 0 ; ...
%     (-K2-(K3*K6)/K4) (-K1) 0 ((K3*K5)/K4) 0 0 ; ...
%     0 0 0 1 0 0 ; ...
%     (K6/K4) 0 0 (-K5/K4) 0 0 ; ...
%     0 0 0 0 0 1 ; ...
%     0 0 0 0 0 (-K8/K7)];

A = [0 1 0 0 ; ...
    (-K2-(K3*K6)/K4) (-K1) ((K3*K5)/K4) 0 ; ...
    (K6/K4) 0 (-K5/K4) 0 ; ...
    0 0 0 (-K8/K7)];

% B = [0 0; ...
%     (-K3/K4) 0; ...
%     0 0; ...
%     (1/K4) 0; ...
%     0 0; ...
%     0 (1/K7)];

B = [0 0; ...
    (-K3/K4) 0; ...
    (1/K4) 0; ...
    0 (1/K7)];

% C = [1 0 0 0 0 0; ...
%     0 0 1 0 0 0; ...
%     0 0 0 1 0 0; ...
%     0 0 0 0 1 0; ...
%     0 0 0 0 0 1];

C = [0 lbar 1 0; ...
     0 0 0 1];

Co = [1 0 0 0; ...
      0 0 1 0; ...
      0 0 0 1];

D = 0;

%% Poles

Poles = eig(A);

%% Controllability

Control = [B A*B A^2*B A^3*B A^4*B A^5*B];
rankControl = rank(Control);

%% Observability

Observe = [C ; C*A ; C*A^2 ; C*A^3 ; C*A^4 ; C*A^5];
rankObserve = rank(Observe);
Observeo = [Co ; Co*A ; Co*A^2 ; Co*A^3 ; Co*A^4 ; Co*A^5];
rankObserveo = rank(Observeo);

%% Expanded System 

Ae = [A zeros(4,2); ...
      C zeros(2,2)];

Be = [B ; ...
      zeros(2,2)];

% Real pole placement
p = [-1.55 -1.55 -6 -6 -7 -8];
Fe = -place(Ae,Be,p)
F = Fe(1:2,1:4);
Fi = Fe(1:2,5:6);
Realpoles = eig(Ae+Be*Fe)

%LQR
% q = [1/(0.079)^2 1/(1)^2 1/(1.02)^2 1/(0.23)^2 1/(0.27)^2 1/(0.001)^2];
% Q = diag(q);
% r = [1/(10.8)^2 1/(10)^2];
% R = diag(r);
% Fe = -lqr(Ae,Be,Q,R);
% F = Fe(1:2,1:4);
% Fi = Fe(1:2,5:6)
% LQRpoles = eig(Ae+Be*Fe)

%% Observer

L = -place(A',Co',6*eig(A+B*F))';

%% Feedforward

N = [72 0; 0 69];

%Atilt = [A+B*F+L*Co B*Fi;zeros(2,6)];
%Ctilt = [F Fi];
%MtiltNinv= -place(Atilt',Ctilt',eig(Ae+Be*Fe))';
%N = MtiltNinv(5:6,1:2)^-1;
%M = MtiltNinv(1:4,1:2)*N;
